/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "fc_sensors.h"
#include "filters.h"
#include "fc_state.h"
#include "icm42688p_read_polling.h"

#define DBG_TAG "acc"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

extern float F1_wave[12];

biquadFilter_t acce_lpf1[3];
biquadFilter_t acce_lpf2[3];

bool acc_lpf_init = false;
bool acc_use_lpf = true;

void sensor_acc_lpf_init(void)
{
    if (imu_offset()->acc_cutoff == 0)
        imu_offset()->acc_cutoff = ACC_LPF_CUTOFF;
    if (imu_offset()->acc_cutoff > 500)
        imu_offset()->acc_cutoff = 400;

    LOG_I("ACC LPF CUTOFF %dHz\n", imu_offset()->acc_cutoff);
    for (uint8_t i = 0; i < 3; i++)
    {
        biquadFilterInitLPF(&acce_lpf1[i], imu_offset()->acc_cutoff, 1000.f);
        // biquadFilterInitLPF(&acce_lpf2[i], 50, 1000.f);
    }
}

void sensor_acc_init(uint16_t cutoff)
{
    if (cutoff)
    {
        if (cutoff < 500)
        {
            imu_offset()->acc_cutoff = cutoff;
            imu_offset()->flag = IMU_SAVED_FLAG;
            zino_config_save_all();
        }
    }
    else
    {
        if (imu_offset()->flag == IMU_SAVED_FLAG)
        {
            fc_sensor_data.acc.acc_offset.isdatacalibrated = imu_offset()->acc_calibrated;
            fc_sensor_data.acc.acc_offset.zero.x = imu_offset()->accoffset.x;
            fc_sensor_data.acc.acc_offset.zero.y = imu_offset()->accoffset.y;
            fc_sensor_data.acc.acc_offset.zero.z = imu_offset()->accoffset.z;
            fc_sensor_data.acc.acc_offset.samplesize = 0;
            LOG_I("acc offset: %d %d %d\n", fc_sensor_data.acc.acc_offset.zero.x, fc_sensor_data.acc.acc_offset.zero.y, fc_sensor_data.acc.acc_offset.zero.z);
        }
        else
        {
            imu_offset()->acc_cutoff = ACC_LPF_CUTOFF;
        }
        if (imu_offset()->acc_cutoff >= 500 || imu_offset()->acc_cutoff <= 0)
        {
            imu_offset()->acc_cutoff = ACC_LPF_CUTOFF;
        }
        LOG_I("ACC LPF CUTOFF %dHz\n", imu_offset()->acc_cutoff);

        fc_sensor_data.acc.scacle = get_acc_scale_factor();
        rt_kprintf_float("scale: ", fc_sensor_data.acc.scacle, " \n");
        for (uint8_t i = 0; i < 3; i++)
        {
            biquadFilterInitLPF(&acce_lpf1[i], imu_offset()->acc_cutoff, 1000.f);
        }
    }
}

void AccStartCalibration(void)
{
    fc_sensor_data.acc.acc_offset.isdatacalibrated = false;
    fc_sensor_data.acc.acc_offset.samplesize = ACC_GYRO_CALIBRATE_SAMPLE;
    FC_STATE_SET(FC_IMU_CALIBRATING);
}
void sensor_acc_calibrate(acc_t *acc)
{
    if (acc->acc_offset.samplesize != 0)
    {
        if (acc->acc_offset.samplesize == ACC_GYRO_CALIBRATE_SAMPLE)
        {
            acc->acc_offset.samplesum.x = 0;
            acc->acc_offset.samplesum.y = 0;
            acc->acc_offset.samplesum.z = 0;
        }

        acc->acc_offset.samplesum.x += acc->accRaw.x;
        acc->acc_offset.samplesum.y += acc->accRaw.y;
        acc->acc_offset.samplesum.z += acc->accRaw.z;

        if (acc->acc_offset.samplesize == 1)
        {
            acc->acc_offset.zero.x = acc->acc_offset.samplesum.x / ACC_GYRO_CALIBRATE_SAMPLE;
            acc->acc_offset.zero.y = acc->acc_offset.samplesum.y / ACC_GYRO_CALIBRATE_SAMPLE;
            acc->acc_offset.zero.z = acc->acc_offset.samplesum.z / ACC_GYRO_CALIBRATE_SAMPLE;

            acc->scacle = get_acc_scale_factor();
            acc->acc_offset.isdatacalibrated = true;

            imu_offset()->flag = IMU_SAVED_FLAG;
            imu_offset()->accoffset.x = acc->acc_offset.zero.x;
            imu_offset()->accoffset.y = acc->acc_offset.zero.y;
            imu_offset()->accoffset.z = acc->acc_offset.zero.z;
            imu_offset()->acc_calibrated = true;
            if(imu_offset()->acc_cutoff == 0)
                imu_offset()->acc_cutoff = ACC_LPF_CUTOFF;
            zino_config_save_all();
            FC_STATE_CLEAR(FC_IMU_CALIBRATING);
        }
        acc->acc_offset.samplesize--;
        // rt_kprintf("ACC CALIBRATING %d\n", acc->acc_offset.samplesize);
    }
}
void sensor_acc_process(acc_t *acc)
{
    if (!imu_offset()->acc_calibrated || !acc->acc_offset.isdatacalibrated)
    {
        //**First boot calibration */
        if (!imu_offset()->acc_calibrated)
        {
            imu_offset()->accoffset.x = 0;
            imu_offset()->accoffset.y = 0;
            imu_offset()->accoffset.z = 0;
        }
        if (acc->acc_offset.samplesize == 0)
        {
            AccStartCalibration();
            LOG_D("First boot calibration");
        }
        /**perform calibration */
        sensor_acc_calibrate(acc);
    }

    acc->accdata.x = (float)(acc->accRaw.x - acc->acc_offset.zero.x) * acc->scacle;
    acc->accdata.y = (float)(acc->accRaw.y - acc->acc_offset.zero.y) * acc->scacle;
    acc->accdata.z = (float)(acc->accRaw.z - acc->acc_offset.zero.z) * acc->scacle + 1.0f;

    // rt_printf_float("acc: ", fc_sensor_data.acc.accdata.x, ", ");
    // rt_printf_float(" ", fc_sensor_data.acc.accdata.y, ", ");
    // rt_printf_float(" ", fc_sensor_data.acc.accdata.z, "\r\n");
    // LOG_D("raw: %d ", acc->accRaw.x);
    // rt_printf_float("acc_offset: ", acc->acc_offset.zero.x, " ");
    // rt_printf_float("scale: ", acc->scacle, " ");
    // rt_printf_float("acc: ", acc->accdata.x, " ");
    for (uint8_t i = 0; i < 3; i++)
    {
        if (acc_use_lpf)
        {
            acc->acclpf.axis[i] = biquadFilterApply(&acce_lpf1[i], acc->accdata.axis[i]);
            // acc->acclpf.axis[i] = biquadFilterApply(&acce_lpf2[i], acc->acclpf.axis[i]);
        }
        else
        {
            acc->acclpf.axis[i] = acc->accdata.axis[i];
        }
        acc->accSum.axis[i] += acc->acclpf.axis[i];
        acc->sumCount.axis[i]++;
    }
    // rt_kprintf("lpf: ", acc->acclpf.x, " \n");
}
void acc_trace(fc_sensor_data_t *sensor, zdrone_state_t *state, float dt)
{
#define ACC_NOSISE 0.004f
    // fc_sensor_data.acc.accSum.x += ((ax > -ACC_NOSISE && ax < ACC_NOSISE) ? 0 : ax); // 1/2*a*t^2
    // fc_sensor_data.acc.accSum.y += ((ay > -ACC_NOSISE && ay < ACC_NOSISE) ? 0 : ay);
    // fc_sensor_data.acc.accSum.z += ((az > -ACC_NOSISE && az < ACC_NOSISE) ? 0 : az);

    // {
    // fc_sensor_data.acc.accSum.x = 0;
    // fc_sensor_data.acc.accSum.y = 0;
    // fc_sensor_data.acc.accSum.z = 0;
    // fc_sensor_data.acc.sumCount.x = 0;
    for (uint8_t i = 0; i < 3; i++)
    {
        if (FC_STATE_CHECK(FC_IMU_CALIBRATING))
        {
            sensor->acc.sumCount.axis[i] = 0;
            sensor->acc.accSum.axis[i] = 0;
        }
        else
        {
            // state->acc.axis[i]=constrainf(state->acc.axis[i],-1.0f,1.0f);
            sensor->acc.sumCount.axis[i]++;
            sensor->acc.accSum.axis[i] += ((state->acc.axis[i] > -ACC_NOSISE && state->acc.axis[i] < ACC_NOSISE) ? 0 : state->acc.axis[i]);
        }
    }
}
void acc_update(void)
{
    // icm42688p_accel_read_polling2(fc_sensor_data.acc.accRaw.axis);
    sensor_imc42688p_poll_data(&fc_sensor_data);
    sensor_acc_process(&fc_sensor_data.acc);
}

void imu_reset_config()
{
    imu_offset()->acc_cutoff = ACC_LPF_CUTOFF;
    imu_offset()->gyro_cutoff = GYRO_LPF_CUTOFF;
}
